#include "ros/ros.h"
#include "cv_bridge/cv_bridge.h"
#include <opencv2/opencv.hpp>
#include "image_transport/image_transport.h"

void imageCallbackRaw(const sensor_msgs::ImageConstPtr &msg)
{
    try
    {
        cv::imshow("raw_image", cv_bridge::toCvShare(msg, "bgr8")->image);
    }
    catch (const std::exception &e)
    {
        ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
    }
}

void imageCallbackColored(const sensor_msgs::ImageConstPtr &msg)
{
    try
    {
        cv::imshow("colored_image", cv_bridge::toCvShare(msg, "bgr8")->image);
    }
    catch (const std::exception &e)
    {
        ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
    }
}

void imageCallbackDepth(const sensor_msgs::ImageConstPtr &msg)
{
    try
    {
        cv::imshow("depth_image", cv_bridge::toCvShare(msg, "bgr8")->image);
    }
    catch (const std::exception &e)
    {
        ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "subscriber");

    cv::namedWindow("raw_image");
    cv::namedWindow("colored_image");
    cv::startWindowThread();
    ros::NodeHandle nh;
    image_transport::ImageTransport it(nh);
    image_transport::Subscriber sub_raw_image = it.subscribe("/camera/rgb/image_color", 1, imageCallbackRaw);
    image_transport::Subscriber sub_colored_image = it.subscribe("/classifier/image_colored", 1, imageCallbackColored);
    image_transport::Subscriber sub_depth_image = it.subscribe("/classifier/image_depth_copy", 1, imageCallbackDepth);

    ros::spin();
    cv::destroyWindow("view");
}